#include <stdio.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "iot_gpio_ex.h"
#include "motor_control.h"

#define PWM_INTERVAL_TIME_US 300000
#define PWM_TASK_STACK_SIZE 512*4
#define PWM_TASK_PRIO 20
#define PWM_OUT_GPIO1 6
#define PWM_OUT_GPIO2 7
extern int action_cur;
extern int action_finish;
extern int last_action;
extern int action_num;
//KEY
#define IOT_GPIO_KEY 5

typedef struct 
{
    int port;
    int max,min;
    int control_num;
    int control_state;
    int position;
    int positionIinit;
    int numMax;

}pwm_control;
pwm_control steer1,steer2;

typedef struct 
{
    int steer1InitFlag;
    int steer2InitFlag;
    int steer1Position1;
    int steer1Position2;

    int steer2Position1;
    int steer2Position2;

    int control_num;
    int state;
    int steer_target;
    int finish_flag;

}action_control;
action_control action[5]={{0}};

static int g_keyState = 0;
static int g_keyiState = 0;
int target=1;

void steer_control(pwm_control* steer,int position){//position为占空比比例
    if(steer->control_num < steer->numMax){
        if(position<steer->max+1&&position>steer->min-1){
            Low_frePwmStart(steer->port, position, 200);
        }
    }else{
        steer->control_state = 1;
        IoTGpioSetOutputVal(steer->port, 0);
    }
}
//摆手
void action1(action_control* action){
    target = action->steer_target;
    switch(action->state){
        case 0:
            if(!action->steer1InitFlag){
                steer1.control_num=0;
                action->steer1InitFlag = 1;
                steer1.control_state = 0;
            }
            steer1.position = action->steer1Position1;

            if(steer1.control_state==1){
                action->state = 1;
                action->steer1InitFlag = 0;
                steer1.control_num=0;
                steer1.control_state = 0;
            }
        break;

        case 1:
            steer1.position = action->steer1Position2;
            if(steer1.control_state==1){
                action->state = 2;
                steer1.control_num=0;
                steer1.control_state = 0;
            }
        break;
        case 2:
            steer1.position = action->steer1Position1;
            if(steer1.control_state==1){
                action->state = 3;
                steer1.control_num=0;
                steer1.control_state = 0;
            }
        break;
        case 3:
            steer1.position = action->steer1Position2;
            if(steer1.control_state==1){
                action->state = 4;
                steer1.control_num=0;
                steer1.control_state = 0;
            }
        break;
        case 4:
            steer1.position = action->steer1Position1;
            if(steer1.control_state==1){
                action->state = 5;
                steer1.control_num=0;
                steer1.control_state = 0;
            }
        break;
        case 5:
            steer1.position = action->steer1Position2;
            if(steer1.control_state==1){
                action->state = 6;
                steer1.control_num=0;
                steer1.control_state = 0;
            }
        break;
        case 6:
            steer1.position = action->steer1Position1;
            if(steer1.control_state==1){
                action->state = 7;
                steer1.control_num=0;
                steer1.control_state = 0;
            }
        break;
        case 7:
            steer1.position = action->steer1Position2;
            if(steer1.control_state==1){
                action->state = 8;
                steer1.control_num=0;
                steer1.control_state = 0;
                action_finish = 1;
                printf("finish!\n");
            }
        break;
    }
    
}
//前后摇手
void action2(action_control* action){
    switch(action->state){
        case 0:
            if(!action->steer2InitFlag){
                steer2.control_num=0;
                action->steer2InitFlag = 1;
                steer2.control_state = 0;
                
            }
            steer2.position = 20;

            if(steer2.control_state==1){
                action->state = 1;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;

        case 1:
            steer2.position = action->steer2Position1;;
            if(steer2.control_state==1){
                action->state = 2;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 2:
            steer2.position = action->steer2Position2;
            if(steer2.control_state==1){
                action->state = 3;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 3:
            steer2.position = action->steer2Position1;
            if(steer2.control_state==1){
                action->state = 4;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 4:
            steer2.position = action->steer2Position2;
            if(steer2.control_state==1){
                action->state = 5;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 5:
            steer2.position = action->steer2Position1;
            if(steer2.control_state==1){
                action->state = 6;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 6:
            steer2.position = action->steer2Position2;
            if(steer2.control_state==1){
                action->state = 7;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 7:
            steer2.position = action->steer2Position1;
            if(steer2.control_state==1){
                action->state = 8;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 8:
            steer2.position = 20;
            if(steer2.control_state==1){
                action->state = 9;
                steer2.control_num=0;
                steer2.control_state = 0;
                action_finish = 1;
                printf("finish!\n");
                action->steer2InitFlag=0;
            }
        break;
    }

}

void action3(action_control* action){

    switch(action->state){
        case 0:
            if(!action->steer1InitFlag){
                steer1.control_num=0;
                action->steer1InitFlag = 1;
                steer1.control_state = 0;
            }
            steer1.position = action->steer1Position1;

            if(steer1.control_state==1){
                action->state = 1;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;

        case 1:
            steer2.position = 30;
            if(steer2.control_state==1){
                action->state = 2;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 2:
            steer2.position = 10;
            if(steer2.control_state==1){
                action->state = 3;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 3:
            steer2.position = 30;
            if(steer2.control_state==1){
                action->state = 4;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 4:
            steer2.position = 10;
            if(steer2.control_state==1){
                action->state = 5;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 5:
            steer2.position = 30;
            if(steer2.control_state==1){
                action->state = 6;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 6:
            steer2.position = 10;
            if(steer2.control_state==1){
                action->state = 7;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 7:
            steer2.position = 30;
            if(steer2.control_state==1){
                action->state = 8;
                steer2.control_num=0;
                steer2.control_state = 0;
            }
        break;
        case 8:
            steer2.position = 20;
            if(steer2.control_state==1){
                action->state = 9;
                steer1.control_num=0;
                steer1.control_state = 0;
                action_finish = 1;
                action->steer1InitFlag=0;
                printf("finish!\n");
            }
        break;
    }
    
}

static void *PwmTask(const char *arg)
{
    (void)arg;

    steer1.control_num = 0;
    steer2.control_num = 0;
    while (1) {
        if(last_action!=action_cur){
            action[0].state = 0;
            action[1].state = 0;
            action[2].state = 0;
            steer1.control_num = 0;
            steer2.control_num = 0;
            switch(action_cur){
                case 1://担忧
                    steer2.position = 22;
                    steer2.numMax=100;
                    steer1.numMax=40;
                    action[0].steer1Position2=20;
                    action[0].steer1Position1=25;
                break;

                case 2://小幅度前后中速摆动
                    steer1.position = 22;  
                    steer2.numMax=50;
                    action[1].steer2Position2=15;
                    action[1].steer2Position1=25;
                break;

                case 3://小幅度前后快速摆动
                    steer1.position = 27;   
                    steer2.control_num = 0;
                    steer2.numMax=25;
                    action[1].steer2Position2=15;
                    action[1].steer2Position1=25;
                break;

                case 4://小幅度前后快速摆动
                    steer2.position = 15;
                    steer2.numMax=100;
                    steer1.numMax=20;
                    action[0].steer1Position2=20;
                    action[0].steer1Position1=27;
                break;

            }
        }
        steer1.control_num++;
        steer2.control_num++;
        switch(action_cur){
            case 0:
            steer2.position = steer2.positionIinit;
            steer1.position = steer1.positionIinit;
            action[0].state = 0;
            action[1].state = 0;
            action[2].state = 0;
            action_finish = 1;
            break;
            case 1:
                action1(&action[0]);
            break;

            case 2: 
                action2(&action[1]); 
            break;

            case 3:
                action2(&action[1]); 
            break;

            case 4:
                action1(&action[0]);
            break;
            
        }
        // if(g_keyState){
        //     steer1.position = steer1.positionIinit;
        // }else{
        //     steer1.position = 18;
        // }

        steer_control(&steer2,steer2.position);
        steer_control(&steer1,steer1.position);



        if (g_keyiState == 0xffff) {
            g_keyiState = 0;
            break;
        }

        TaskMsleep(1);
    }
}

//按键中断
extern int action_class[5];
static void OnButtonPressed(const char *arg)
{
    (void) arg;
    g_keyState = !g_keyState;
        steer1.control_num = 0;
        steer2.control_num = 0;
        // if(g_keyState){
        //     action_cur = 1;
        // }else{
        //     action_cur = 0;
        // }
        action_cur = 0;

    action_class[1]+=1;
    action_class[2]+=2;
    action_class[3]+=3;
    action_class[4]+=4;
    //steer1.control_num=0;
    //printf("PRESS_KEY!\n");
    g_keyiState++;
    action_num++;
}


void GPIO_Init(void){
    //按键
    IoTGpioInit(IOT_GPIO_KEY);
    IoSetFunc(IOT_GPIO_KEY, 0);
    IoTGpioSetDir(IOT_GPIO_KEY, IOT_GPIO_DIR_IN);
    IoSetPull(IOT_GPIO_KEY, IOT_IO_PULL_UP);
    IoTGpioRegisterIsrFunc(IOT_GPIO_KEY, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_FALL_LEVEL_LOW, OnButtonPressed, NULL);

    steer1.port = PWM_OUT_GPIO1;
    steer1.max = 27;
    steer1.min = 18;
    steer1.numMax = 20;
    steer1.positionIinit = 27;
    steer1.position = steer1.positionIinit;
    steer1.control_num = 0;
    steer1.control_state = steer1.max;

    steer2.port = PWM_OUT_GPIO2;
    steer2.max = 30;
    steer2.min = 10;
    steer2.numMax = 120;
    steer2.position = 20;
    steer2.positionIinit = 20;
    steer2.control_num = 0;
    steer2.control_state = (steer1.max-steer1.min)/2;

    //PWM OUT
    IoTGpioInit(PWM_OUT_GPIO1);
    IoSetFunc(PWM_OUT_GPIO1 , 0); /* 将GPIO10定义为普通GPIO功能*/
    IoTGpioSetDir(PWM_OUT_GPIO1 , IOT_GPIO_DIR_OUT); /* 设置GPIO10方向为输出*/

    IoTGpioInit(PWM_OUT_GPIO2);
    IoSetFunc(PWM_OUT_GPIO2 , 0); /* 将GPIO10定义为普通GPIO功能*/
    IoTGpioSetDir(PWM_OUT_GPIO2 , IOT_GPIO_DIR_OUT); /* 设置GPIO10方向为输出*/

    action[0].steer1Position1 = 21;
    action[0].steer1Position2 = steer1.max;
    action[1].steer1Position1 = 30;
    action[1].steer1Position2 = 10;
    action[2].steer1Position1 = steer1.min;
    action[2].steer1Position2 = steer1.max;
}

static void PwmTaskEntry(void)
{
    osThreadAttr_t attr;


    GPIO_Init();
    IoTWatchDogDisable();
    attr.name = "PwmTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = PWM_TASK_STACK_SIZE;
    attr.priority = PWM_TASK_PRIO;

    if (osThreadNew((osThreadFunc_t)PwmTask, NULL, &attr) == NULL) {
        printf("[LedExample] Falied to create PwmTask!\n");
    }
}

SYS_RUN(PwmTaskEntry);
